#include <Servo.h>
#define left_one 12  //小车的左轮子  连接 10,11号脚
#define left_two 13
#define right_one 2 //小车的右轮子  连接 12,13号脚
#define right_two 4
#define LedRed 3  //led的引脚,3、5、6分别控制红绿蓝三色
#define LedGreen 5
#define LedBlue 6
#define Echo 7 //引脚Echo 连接 7号脚 
#define Trig 8 //引脚Tring 连接 8号脚
#define SpeedPWM 11//电机pwm连接 11号脚
 
/*Led颜色信号*/
#define SetLedGreen 'N'
#define SetLedRed 'D'
#define SetLedBlue 'U'
#define LedSignal 'E'
/*舵机信号*/
#define ServoSignal 'O'
/*超声波测试距离信号*/
#define DistanceSignal 'T'
/*加速减速信号*/
#define SpeedSignal 'P'
/*小车运动信号*/
#define Forward 'F'
#define Back 'B'
#define Left 'L'
#define Right 'R'
#define Stop 'S'
/*Led的开关信号*/
#define LedOn 'K'
#define LedOff 'G'



Servo myservo;  //舵机
String inString = ""; // 存储输入的字符串
long comdata = 0;
float temp;
static int speedpwm=0;//小车速度
static int Led_R=0;
static int Led_G=0;
static int Led_B=0;

/*小车转向*/
void forward();
void backward();
void turnLeft();
void turnRight();
void goStop();

/*Led变换颜色*/
void LedColorChange(int Led_R,int Led_G,int Led_B); 
void LedShow();
 
void setup() {
  Serial.begin(9600);
  myservo.attach(9);  //设置舵机的引脚为9号脚
  myservo.write(90);
  pinMode(left_one, OUTPUT);
  pinMode(left_two, OUTPUT);
  pinMode(right_one, OUTPUT);
  pinMode(right_two, OUTPUT);
  pinMode(LedRed,OUTPUT);
  pinMode(LedGreen,OUTPUT);
  pinMode(LedBlue,OUTPUT);
  pinMode(Trig, OUTPUT);
  pinMode(Echo, INPUT);
  pinMode(SpeedPWM,OUTPUT);
}
 
 
void loop() {
    while (Serial.available() > 0) {
        int inChar = Serial.read();
        switch(inChar)
        {
            /*控制Led颜色设置为红、蓝、绿*/
            case SetLedBlue:
                Led_B=0;
                Led_R=255;
                Led_G=255;
                LedShow();
                break;
            case SetLedGreen:
                Led_B=255;
                Led_R=255;
                Led_G=0;
                LedShow();
                break;
            case SetLedRed:
                Led_B=255;
                Led_R=0;
                Led_G=255;
                LedShow();
                break;
            /*小车的前进左转右转后退*/
            case Forward:
                forward();
                break;
            case Back:
                backward();
                break;
            case Left:
                turnLeft();
                break;
            case Right:
                turnRight();
                break;
            case Stop:
                goStop();
                break;
            /*打开Led灯*/
            case LedOn:
                Led_R = 0;
                Led_G = 0;
                Led_B = 0;
                LedShow();
                break;
            /*关闭Led灯*/
            case LedOff:
                Led_R = 255;
                Led_G = 255;
                Led_B = 255;
                LedShow();
                break;
            default:
                /*接收到控制Led的信号*/
                if ((char)inChar == LedSignal) {
                do {
                    inChar = Serial.read();
                    inString += (char)inChar;
                  }
                while (isDigit(inChar));
                comdata=inString.toInt();
                LedColorChange(comdata/1000000,comdata%1000000/1000,comdata%1000);
                inString = "";
                 }
                /*接收到控制舵机的信号*/
                else if ((char)inChar == ServoSignal) {
                do {
                    inChar = Serial.read();
                    inString += (char)inChar;
                  }
                while (isDigit(inChar));
                myservo.write(abs(180-inString.toInt()));  //小车的舵机转向
                float distance = rangDistance(); //回传超声波模块测试的距离
                Serial.print(DistanceSignal);
                Serial.print(distance);
                inString = "";
                 }
                else if ((char)inChar == SpeedSignal) {
                do {
                    inChar = Serial.read();
                    inString += (char)inChar;
                  }
                while (isDigit(inChar));
                speedpwm = inString.toInt()*2.55;  //设置小车速度
                analogWrite(SpeedPWM,speedpwm);
                inString = "";
                 }
                inString = "";
                LedShow();
                break;     
        }
    } 
    delay(200);
}

/*显示Led的颜色*/
void LedShow()
{
   analogWrite(LedRed,Led_R);
   analogWrite(LedGreen,Led_G);
   analogWrite(LedBlue,Led_B);
}

/*更改Led的颜色*/
void LedColorChange(int R,int G,int B)
{
  int R_temp=abs(255-R);
  int G_temp=abs(255-G);
  int B_temp=abs(255-B);
  if(abs(Led_R-R_temp)<30 and abs(Led_G-G_temp)<30 and abs(Led_B-B_temp)<30)
  {
    Led_R = R_temp;
    Led_G = G_temp;
    Led_B = B_temp;
    LedShow();
  }  
}


/*小车的前进、后退、左转、右转、停止*/
void forward()
{
  digitalWrite(left_one, HIGH);    
  digitalWrite(left_two, LOW);     
  digitalWrite(right_one, HIGH);     
  digitalWrite(right_two, LOW); 
}

void backward()
{
  digitalWrite(left_one, LOW);    
  digitalWrite(left_two, HIGH);     
  digitalWrite(right_one, LOW);     
  digitalWrite(right_two, HIGH); 
}

void turnLeft()
{
  digitalWrite(left_one, HIGH);    
  digitalWrite(left_two, LOW);     
  digitalWrite(right_one, LOW);     
  digitalWrite(right_two, HIGH);
}

void turnRight()
{
  digitalWrite(left_one, LOW);    
  digitalWrite(left_two, HIGH);     
  digitalWrite(right_one, HIGH);     
  digitalWrite(right_two, LOW);
}

void goStop()
{
  digitalWrite(left_one, LOW);    
  digitalWrite(left_two, LOW);     
  digitalWrite(right_one, LOW);     
  digitalWrite(right_two, LOW); 
}

/*超声波测距*/
float rangDistance()
{
  //给Trig发送一个低高低的短时间脉冲,触发测距
  digitalWrite(Trig, LOW); //给Trig发送一个低电平
  delayMicroseconds(2);    //等待 2微妙
  digitalWrite(Trig,HIGH); //给Trig发送一个高电平
  delayMicroseconds(10);    //等待 10微妙
  digitalWrite(Trig, LOW); //给Trig发送一个低电平
  temp = float(pulseIn(Echo, HIGH)); //存储回波等待时间,
  //pulseIn函数会等待引脚变为HIGH,开始计算时间,再等待变为LOW并停止计时
  //返回脉冲的长度
  //声速是:340m/1s 换算成 34000cm / 1000000μs => 34 / 1000
  //因为发送到接收,实际是相同距离走了2回,所以要除以2
  //距离(厘米)  =  (回波时间 * (34 / 1000)) / 2
  //简化后的计算公式为 回波时间 * 0.017
  float cm = temp * 0.017; //把回波时间换算成cm
  return cm;
}


